#include "dkglOBB.h"
#include "dkglBox.h"
#include "dkglTestIntersection.h"

namespace DKGL
{
	namespace bounding
	{

		OrientedBoundingBox::OrientedBoundingBox(float sizex, float sizey, float sizez):
			_size(sizex, sizey, sizez),
			_show(false),
			_box(NULL),
			_worldPosition(Vector3::ZERO)
		{
			_boxMat = MaterialPtr(new Material("XX"));
			_boxMat->setColor(Colorf::PURPLE);
			_boxMat->setRenderMode(RM_WIRE);

			physics::AddOrientedBoundingBox(this);
		}


		OrientedBoundingBox::~OrientedBoundingBox()
		{
			delete _box;
		}


		void OrientedBoundingBox::show(bool b)
		{
			_show = b;
			
			// ondemand allocation
			if (_show)
			{
				if (!_box)
				{
					_box = new primitive::Box(
						"OBB", _size.x, _size.y, _size.z);
					_box->setMaterial(0, _boxMat);
				}
			}
		}


		Vector3 OrientedBoundingBox::getCenter() const
		{
			return _worldPosition;
		}


		bool OrientedBoundingBox::testIntersection(
				const OrientedBoundingBox& a,
				const OrientedBoundingBox& b
				)
		{
			int i, j;

			for (i=0;i<3;i++)
			{
				// testing with a's axes
				if (!testSeparatingAxis(a.getAxis(i), a, b))
					return false;// disjoint

				// testing with b's axis
				if (!testSeparatingAxis(b.getAxis(i), a, b))
					return false;// disjoint
			}

			Vector3 sepAxis;

			for (i=0;i<3;i++)
			{
				for (j=0;j<3;j++)
				{
					sepAxis = a.getAxis(i).crossProduct(b.getAxis(j));
					if (!testSeparatingAxis(sepAxis, a, b))
						return false;// disjoint
				}
			}

			return true;
		}

		bool OrientedBoundingBox::testSeparatingAxis(
				const Vector3& sepAxis,
				const OrientedBoundingBox& a,
				const OrientedBoundingBox& b
				)
		{
			Vector3 t = b.getCenter() - a.getCenter();
			float tl = t.dotProduct(sepAxis);
			float da = a.getDN(sepAxis);
			float db = b.getDN(sepAxis);

			if (fabs(tl) > da + db)
				return false; // disjoint
			else
				return true;

		}

		float OrientedBoundingBox::getDN(const Vector3& v) const
		{
			float r = 0.0f;

			for (int i=0;i<3;i++)
			{
				r += _size[i]*0.5f*_worldAxis[i].dotProduct(v);
			}

			return r;
		}


		void OrientedBoundingBox::setWorldTransformation(const float* world)
		{
			_worldAxis[0] = Vector3(world[0], world[1], world[2]);
			_worldAxis[1] = Vector3(world[4], world[5], world[6]);
			_worldAxis[2] = Vector3(world[8], world[9], world[10]);
			_worldPosition = Vector3(world[12], world[13], world[14]);
			_box->setWorldTransformation(world);
		}


		Drawable* OrientedBoundingBox::getDrawable() const
		{
			//show(true);
			return _box;
		}
	}
}